#include "focus_search.h"
#include <QDebug>

ClimbSearch::ClimbSearch() :
	initialDirection(false),
	direction(initialDirection),
	initialStep(400),
	step(initialStep),
	isFinished(false),
	threshold(5)
{
	
}


FocusAction ClimbSearch::search(const QQueue<double>& focusValues)
{

#if 0
	if (focusValues.size() != 3) {
		return FocusAction(FocusAction::STOP, 0.0);
	}

	if (abs(focusValues[1] - focusValues[0]) > threshold) {
		direction = initialDirection;
		step = initialStep;
	}

	auto dir = [](bool direction) {
		return !direction ? FocusAction::BACKWARD : FocusAction::FORWARD;
	};

	isBorder = focusValues[2] < 0;

	if (isBorder) {
		direction = !direction;
	}
	if (focusValues[2] < focusValues[1]) {
		direction = !direction;
		step /= 2;
	}
	
	return FocusAction(dir(direction), step);
#endif

	if (focusValues.size() != 3) {
		return FocusAction(FocusAction::STOP, 0.0);
	}

	if (abs(focusValues[1] - focusValues[0]) > threshold && focusValues[2] < focusValues[1]) {
		// step = initialStep;
	}
	if (focusValues[2] < threshold) {
		step = initialStep;
	} else if (focusValues[2] >= threshold && focusValues[2] < 2*threshold) {
		step = initialStep / 2;
	}
	else {
		if (step >= initialStep / 2) {
			step = initialStep / 2;
		}
	}

	auto dir = [](bool direction) {
		return !direction ? FocusAction::BACKWARD : FocusAction::FORWARD;
	};

	//isBorder = focusValues[2] < 0;

	if (border) {
		direction = !direction;
	}

#if 1
	if (focusValues[2] > 2*threshold && focusValues[2] < focusValues[1]) {
		direction = !direction;
		// step /= 2;
		step = int(step) / 2;
		if (step <= 0) {
			step = 10;
			if (focusValues[2] > 2.5 * threshold) {
				step = 1;
			}
		}
	}
#endif

#if 0
	if (focusValues[0] < focusValues[1] && focusValues[1] < focusValues[2]) {

	}
	else if (focusValues[0] > focusValues[1] && focusValues[1] > focusValues[2]) {
		direction = !direction;
		step = int(step) / 2;
		if (step <= 0) {
			step = 1;
		}
	}
	else {

	}
#endif

	return FocusAction(dir(direction), step);
}

TraverseSearch::TraverseSearch() :
	FocusSearch(),
	init_step(100),
	step(init_step),
	maxFocusValue(0),
	threshold(5e7),
	finished(false),
	finishedCount(0),
	focusValuesRange(1e64, 0)
{
	
}

void TraverseSearch::setMaxFocus(int focus)
{
	maxFocus = focus;
	focusRange.first = 0;
	focusRange.second = focus;
	searchRange = focusRange;
}


FocusAction TraverseSearch::search(const QQueue<double>& focusValues)
{
	if (focusValues.empty()) {
		return FocusAction(FocusAction::STOP, 0);
	}

	double focus_value = focusValues.back();
#if 0
	if (finished && abs(focus_value - maxFocusValue) > threshold) {
		finished = false;
		step = init_step;
		searchRange = focusRange;
		maxSearchPos = {-1, -1};
		return FocusAction(FocusAction::SEEK, 0);
	}
	if (finished && abs(focus_value - maxFocusValue) < threshold) {
		return FocusAction(FocusAction::STOP, 0);
	}
#endif

	focusValuesRange.first = std::min(focusValuesRange.first, focus_value);
	focusValuesRange.second = std::max(focusValuesRange.second, focus_value);

	if (finished) {
		if (finishedCount < 10) {
			++finishedCount;
			return FocusAction(FocusAction::STOP, 0);
		}

		double rate = focusValuesRange.second / focusValues[2];
		if (rate < 1.0) {
			rate = 1 / rate;
		}
		double proce_rate = focusValuesRange.second / focusValuesRange.first;
		qDebug() << "rate = " << rate << " proce-rate = " << proce_rate;
		if (rate > 2.0 || proce_rate <= 1.5) {
			finished = false;
			step = init_step;
			searchRange = focusRange;
			maxSearchPos = {-1, -1};
			finishedCount = 0;
			focusValuesRange = { 1e64, 0 };
			return FocusAction(FocusAction::SEEK, 0);
		}
		return FocusAction(FocusAction::STOP, 0);
	}

	int newFocusPos = focusPos;

	if (newFocusPos <= searchRange.second) {
		if (maxSearchPos.first < focus_value) {
			maxSearchPos = { focus_value, newFocusPos };
		}
		newFocusPos += step;
		if (newFocusPos > searchRange.second) {
			searchRange.first = std::max(0, maxSearchPos.second - step);
			searchRange.second = std::min(maxFocus, maxSearchPos.second + step);

			bool has_one = step > 1;
			step /= 10;
			if (step == 0 && has_one) {
				step = 1;
			}
			
			newFocusPos = searchRange.first;

			if (step == 0) {
				maxFocusValue = maxSearchPos.first;
				finished = true;
			}

			maxSearchPos = { -1, -1 };

			return FocusAction(FocusAction::SEEK, newFocusPos);
		}
	}

	return FocusAction(FocusAction::FORWARD, step);

	if (step == init_step && newFocusPos <= searchRange.second) {
		if (maxSearchPos.first < focus_value) {
			maxSearchPos = { focus_value, newFocusPos };
		}
		newFocusPos += step;
		if (newFocusPos > searchRange.second) {
			searchRange.first = std::max(0, maxSearchPos.second - step);
			searchRange.second = std::min(maxFocus, maxSearchPos.second + step);
			step /= 10;
			newFocusPos = searchRange.first;
			maxSearchPos = { -1, -1 };

			return FocusAction(FocusAction::SEEK, newFocusPos);
		}
	} else if (step == init_step / 10 && newFocusPos <= searchRange.second) {
		if (maxSearchPos.first < focus_value) {
			maxSearchPos = { focus_value, newFocusPos };
		}
		newFocusPos += step;
		if (newFocusPos > searchRange.second) {
			searchRange.first = std::max(0, maxSearchPos.second - step);
			searchRange.second = std::min(maxFocus, maxSearchPos.second + step);
			step /= 10;
			newFocusPos = searchRange.first;
			maxSearchPos = { -1, -1 };

			return FocusAction(FocusAction::SEEK, newFocusPos);
		}
	} else if (step == init_step / 100 && newFocusPos <= searchRange.second) {
		if (maxSearchPos.first < focus_value) {
			maxSearchPos = { focus_value, newFocusPos };
		}
		newFocusPos += step;
		if (newFocusPos > searchRange.second) {
			searchRange.first = std::max(0, maxSearchPos.second - step);
			searchRange.second = std::min(maxFocus, maxSearchPos.second + step);
			step /= 10;
			newFocusPos = searchRange.first;
			maxFocusValue = maxSearchPos.first;
			maxSearchPos = { -1, -1 };
			finished = true;

			return FocusAction(FocusAction::SEEK, newFocusPos);
		}
	}
	return FocusAction(FocusAction::FORWARD, step);
}

